% main function
% kneed biped, without torso

%angle convention
%q0 - angle the stance calf makes with the vertical, counterclockwise is positive
%q1 - relative stance knee angle,  counterclockwise is positive
%q2 - relative hip angle,          clockwise is positive
%q3 - relative swing knee angle,   counterclockwise is positive
% at q = (0,0,0,0) the biped is standing straight up, with locked knees

% clc;clear all;
% function main_ntorso(OutPutType)
OutPutType = '0';
clear tp tp0 yact ydes p data t
% addpath('./result/OutputCombination1/build_ntorso')
addpath('./wrappers_ntorso')
%Simulation variables
g = 981/100;
numsteps = 10;
tf = 0;
data = cell(numsteps,1);
%Control Gain
ep = 20;
% Parameters


% load('a_opt');
% load('x_opt');
ic = x_opt;
a = a_opt;



%%%%%%(Comment this line if your initial condition is post-impact%%%%%
ic = resetFunc(ic);

%%%Variables used to store output data
global tp tp0 yact ydes p

tp0 = 0;
tp = [];
yact = [];
ydes = [];

%Initial position of the hip
p = phip_sca(ic);
% %%%%%%%%% Fix Hip IC %%%%%%%%%%
% p = a[1,5];
% pause(1000)

%Initialize animation plot
fig=figure(2);
% % aviobj = avifile('o3.avi');%
ramp = plot([-1 10], [0 0], 'b:');
px=0;
py=0;
hold on;
legs = plot([0 0], 'k');
hold off;
axis([-1 10 -1 1.2]);


%
        pos = jpos_mat(ic);
        phipz = pos(1,3)-p(1)-px;
        set(legs, 'XData', pos(1,:), 'YData', pos(2,:), ...
            'erasemode', 'normal');
        axis equal;
        hold on;
        drawnow;
        
%         pause(1000);

%
for i=1:numsteps
    sol = ode45(@(t,x) f1_vector(t,x,a,ep), [0 1], ic, ...
        odeset('Events', @(t,x) eventfcn(t,x,a), 'MaxStep', 1e-2));
    
    T = tf + sol.x;
    data{i} = [T; sol.y];
    ic = resetFunc(sol.y(:,end));
%     ic2 =  sol.y(:,end)
    tf = tf + sol.x(end);
    tp0 = tf;
    
    F = [sol.x; sol.y];
    [m, k] = size(data{i});    
    qo = cat(2, data{:});
    
    %
    for n=1:k
        % position
        x = [px;py;sol.y(:,n)];
        njoints = length(jpos_mat(sol.y(1:4,n)));
        pos = jpos_mat(sol.y(1:4,n))+...
            [px*ones(1,njoints); py*ones(1,njoints)];
%         pos_x(i,:) = pos(1,:);
%         pos_y(i,:) = pos(2,:);
        phipz = pos(1,3)-p(1)-px;
        set(legs, 'XData', pos(1,:), 'YData', pos(2,:), ...
            'erasemode', 'normal');
        axis equal;
        hold on;
        drawnow;
% %         F = getframe(fig);
% %         aviobj = addframe(aviobj,F);
%         pause(0.3);
        
    end
    
    %update positions
%     px = pos(1,end)+px;
%     py = pos(2,end)+py;Charles
    px = pos(1,end);
    py = pos(2,end);
    %update initial position of the hip
%     ph = pos(1,3)-px; 
%     p = ph;  
end
% %        close(fig)
% %        aviobj = close(aviobj);
% % % figname = strcat(OutPutType,'Config.pdf');
% % % saveas(gcf,figname)
%%
%plots
% %angles v. time & limit cycles
q=cat(2,data{:});

%  for n=1:length(q)
%         % position
% %         x = [px;py;sol.y(:,)];
%         pos = jpos_mat(q(2:end,n));
%         phipz = pos(1,3)-p(1)-px;
%         set(legs, 'XData', pos(1,:), 'YData', pos(2,:), ...
%             'erasemode', 'normal');
%         axis equal;
%         hold on;
%         drawnow;
% %         pause(0.05);
%         
%  end
    
%%
fig1 = figure(1); clf;
% subplot(2,1,1);
% plot(q(1,:),q(2,:), 'r-', q(1,:),q(3,:), 'b-',q(1,:),q(4,:),'m',q(1,:),q(5,:),'c');
% subplot(2,1,2);
pp1 = plot(q(2,:),q(6,:), 'g', q(3,:),q(7,:), 'm',...
    q(4,:),q(8,:), 'r',q(5,:),q(9,:), 'b');
set(pp1,'MarkerSize', 10,'LineWidth',3);
% set(gca,'FontSize',12);
% apc = [0.100000000000000   0.2000000000000   0.775000000000000*0.7   0.690000000000000*0.7];
apc = [0.100000000000000   0.2000000000000   0.775000000000000*1.2   0.690000000000000*1.2];
set(gca,'position',apc);
xlabel({'Angle(rad)'},'Interpreter','LaTex','FontSize',20)
ylabel({'Angular Velocity(rad/s)'},'Interpreter','LaTex','FontSize',20)
set(gca,'FontSize',16)
% ah1=gca;
l=legend(gca,pp1,'Subject1',{'$\theta_{sf}$','$\theta_{sk}$',...
    '$\theta_{hip}$','$\theta_{nsk}$'},1, ...
   'Location', 'BestOutside', 'Orientation','horizontal','Interpreter','LaTeX','FontSize',20);
p = get(l,'position');
set(l, 'position', [0 p(2)-.2 1-0.25 p(4)], 'Box','off');
figname = strcat(OutPutType,'LC.eps');
% saveas(fig1,figname)
print(gcf, '-depsc',figname)
%%
save('62q.mat','q');
fig2 = figure(2); clf;
pp2 = plot(q(1,:),q(2,:), 'r-', q(1,:),q(3,:), 'b-',q(1,:),q(4,:),'m',q(1,:),q(5,:),'c');
set(gca,'FontSize',12);
set(pp2,'MarkerSize', 12,'LineWidth',3);
apc = [0.100000000000000   0.2000000000000   0.775000000000000*0.7   0.690000000000000*0.7];
set(gca,'position',apc);
xlabel({'Time(s)'},'Interpreter','LaTex','FontSize',20)
ylabel({'Angle(rad)'},'Interpreter','LaTex','FontSize',20)
l=legend(gca,pp2,'Subject1',{'$\theta_{sf}$','$\theta_{sk}$',...
    '$\theta_{hip}$','$\theta_{nsk}$'},1, ...
   'Location', 'BestOutside', 'Orientation','horizontal','Interpreter','LaTeX','FontSize',20);
p = get(l,'position');
set(gca,'FontSize',14)
set(l, 'position', [0 p(2)-.2 1-0.25 p(4)], 'Box','off');
figname = strcat(OutPutType,'StateVariables.eps');
% saveas(fig2,figname)
print(gcf, '-depsc',figname)

% print -depsc -tiff figname
% ah2=axes('position',get(gca,'position')-[0 .4 0 0], 'visible','off');
% l2=legend(ah2,pp2,{'$\theta_{sl}vs{\dot\theta_{sl}}$','$\theta_{sk}vs{\dot\theta_{sk}}$','$\theta_{hip}vs{\dot\theta_{hip}}$','$\theta_{nsk}vs{\dot\theta_{nsk}}$'},2, ...
%    'Location', 'BestOutside', 'Orientation','horizontal','Interpreter','LaTeX','FontSize',20);
% set(l2, 'position', [0 p(2)-.17 1 p(4)],'Box','off');


%outputs
% figname = strcat(OutPutType,'LC.pdf');
% saveas(fig1,figname)
% 
% clear t g ttau pos_hip p_hip phip actual_tau
for i = 1:length(q)
    t(i) = q(1,i);
    g(i) = h_sca(q(2:end,i));
%     ttau(i) = tau(q(2:end,i),a);
%     phip(i) = P_hip(q(2:end,i));
%     actual_tau(i) = (phip(i)-phip(1))/a(1,1);
end
%%
fig3 = figure(3); clf;
% plot(tp,yact,'k',tp,ydes,'r',t,g,'g')
pp3 = plot(tp,yact(1,:),'k',tp(1:20:end),ydes(1,1:20:end),'rs'...
    ,tp,yact(2,:),'k',tp(1:20:end),ydes(2,1:20:end),'bo'); hold on;
pp4 = plot(tp,yact(3,:),'k',tp(1:20:end),ydes(3,1:20:end),'gd',...
tp,yact(4,:),'k',tp(1:20:end),ydes(4,1:20:end),'mp')
set(pp3,'MarkerSize', 10,'LineWidth',3);
set(pp4,'MarkerSize', 10,'LineWidth',3);
xlim([0 0.61])
% ylim([-0.7 1.2])
apc = [0.100000000000000   0.2000000000000   0.775000000000000*0.7   0.690000000000000*0.7];
set(gca,'position',apc);
% title('ya vs yd')
% xlabel('time (s)');
% ylabel('desired vs. actual')
set(gca,'FontSize',16);
xlabel({'Time(s)'},'Interpreter','LaTex','FontSize',16)
ylabel({'Outputs'},'Interpreter','LaTex','FontSize',16)

l=legend(gca,pp3,'Subject1',{'$\delta p_{hip}^{a}$',...
    '$\delta p_{hip}^{d}$','$\theta_{hip}^a$','$\theta_{hip}^d$'},1, ...
   'Location', 'BestOutside', 'Orientation','horizontal','Interpreter','LaTeX','FontSize',16);
p = get(l,'position');
set(l, 'position', [0 p(2)-.16-0.02 1-0.25 p(4)], 'Box','off');


ah2=axes('position',get(gca,'position')-[0 .5 0 0], 'visible','off');
l2=legend(ah2,pp4,{'$\theta_{sk}^a$','$\theta_{sk}^d$','$\theta_{nsk}^a$','$\theta_{nsk}^d$'},2, ...
   'Location', 'BestOutside', 'Orientation','horizontal','Interpreter','LaTeX','FontSize',16);
set(l2, 'position', [0 p(2)-.22-0.01 1-0.25 p(4)],'Box','off');


% legend('Location','EastOutside',{'y_1 actual','y_2 actual','y_3 actual','y_4 actual','y_1 desired','y_2 desired','y_3 desired','y_4 desired','guard'});
figname = strcat(OutPutType,'yaVSyd.eps');
% saveas(fig3,figname)
print(gcf, '-depsc',figname)
% figure(7); clf;
% plot(t,g,'k')
% xlabel('time (s)');\theta_{sf}\theta_{sf}
% ylabel('Guard')
% legend({'guard'});
% 
% figure(8); clf;
% plot(t,ttau,'k',t,t,'r',t,actual_tau,'b+')
% xlabel('time (s)');
% ylabeCharlesl('Tau')
% legend({'tau'});
% 
% 
% end
